%% SINS+GNSS融合
% TODO: 最后结果不太对，呈现发散趋势
close all;
clear;
clc;

%% 参数初始化
rng(1);
Re = 6378245;                                       %地球半径(m)
OmegaIE = 7.292e-5;                                 %地球自转角速度(rad/s)
gE = 9.8;                                           %地球重力加速度(m/s^2)
deg2rad = 1/180*pi;                                 %角度转弧度

%% 理论数据及观测数据生成
theoryTraj = importdata('theoryOutTraj.txt').data;  %时间+欧拉角+纬经高+东北天速度
theoryIMU = importdata('theoryOutIMU.txt').data;    %时间+加速度计+陀螺仪
measureGNSS = importdata('outputGNSS.txt').data;    %纬经高+东北天速度
measureIMU = importdata('realOutputIMU.txt').data;  %时间+加速度计+陀螺仪
T = 200;
dt = 0.01;
nstep = 100;
% 系统噪声W的方差Q
matQ = zeros(12);
matQ(8:9, 8:9) = eye(2)*((0.03/3600)^2);            %加速度计噪声
matQ(10:12, 10:12) = eye(3)*((0.25/3600)^2);        %陀螺仪噪声
% 测量矩阵H
matH = [eye(4),zeros(4,8)];
% 测量噪声V的方差R
matR = [eye(2)*(3.2^2), zeros(2); zeros(2), eye(2)*(0.2^2)];
% X0和P0
X0 = zeros(12, 1);
P0 = zeros(12);
P0(1,1) = (1/3600*deg2rad)^2;
P0(2,2) = (1/3600*deg2rad)^2;
P0(8:12,8:12) = matQ(8:12,8:12);

%% EKF
estimateXpre = X0;
meanSquareErrorPre = P0;
lastAngIMU = theoryTraj(1, 2:4)' * deg2rad;
lastPosIMU = theoryTraj(1, 5:7)';
lastPosIMU(1) = lastPosIMU(1) * deg2rad;
lastPosIMU(2) = lastPosIMU(2) * deg2rad;
lastVelIMU = theoryTraj(1, 8:10)';
% 画图记录量
time = 1:1:T;
measureZkLog = zeros(4, T);
measureGNSSLog = zeros(4, T);
estimatePosLog = zeros(2, T);
estimateVelLog = zeros(2, T);
estimateAngLog = zeros(3, T);

for i = 2:1:T/dt
    
    % 没有GPS时使用IMU进行积分(latitSINS\lamdaSINS\vnSINS\veSINS)    
    % accelIMU(B)-->velIMU(I)
    velIMU = getVel(measureIMU(i, 2:4)', lastPosIMU, lastVelIMU, dt);
    % omegaIMU(B)-->angIMU(I)
    angIMU = getAtt(lastAngIMU, measureIMU(i, 5:7)', dt);
    % vI-->posIMU
    posIMU = getPos(lastPosIMU, lastVelIMU, velIMU, dt);
    % 赋值
    latitSINS = posIMU(1);
    lamdaSINS = posIMU(2);
    veSINS = velIMU(1);
    vnSINS = velIMU(2);  
    % 给下一步
    lastVelIMU = velIMU;
    lastAngIMU = angIMU;
    lastPosIMU = posIMU;
    
    % 有GPS时进行卡尔曼融合
    if mod(i, nstep) == 0
        j = i / nstep;
        % 相对于KF，EKF的Phi阵要由状态方程f(X)实时求偏导得到
        L = measureGNSS(j, 1) * deg2rad;
        VE = measureGNSS(j, 4);
        VN = measureGNSS(j, 5);
        fE = measureIMU(i, 2);
        fN = measureIMU(i, 3);
        fD = measureIMU(i, 4);
        matPHIk = getPHI(Re, OmegaIE, L, VE, VN, fE, fN, fD, dt);
        % 相对于KF，EKF的一部预测方程使用非线性的状态方程f(X)
        estimateXpre = matPHIk * estimateXpre;
        % 一步预测Pk
        meanSquareErrorPre = matPHIk*meanSquareErrorPre*matPHIk' + matQ;
        % 相对于KF，EKF的Hk阵要由观测方程h(X)实时求偏导得到
        matHk = matH;
        matHkT = matHk';
        % 增益K
        gainK = meanSquareErrorPre*matHkT/(matHk*meanSquareErrorPre*matHkT + matR);
        % X估计，相对于KF，此处使用观测方程h(X)
        hx = matHk * estimateXpre;
        measureZk = zeros(4, 1);
        measureZk(1) = latitSINS - measureGNSS(j, 1)*deg2rad;
        measureZk(2) = lamdaSINS - measureGNSS(j, 2)*deg2rad;
        measureZk(3) = vnSINS - measureGNSS(j, 5);
        measureZk(4) = veSINS - measureGNSS(j, 4);
        estimateX = estimateXpre + gainK*(measureZk - hx);
        % P估计
        tmp = eye(12) - gainK*matHk;
        meanSquareErrorK = tmp*meanSquareErrorPre*tmp' + gainK*matR*gainK';
    
        estimateXpre = estimateX;
        meanSquareErrorPre = meanSquareErrorK;
        % 用GNSS校正IMU积分
        lastPosIMU(1) = measureGNSS(j, 1)*deg2rad;
        lastPosIMU(2) = measureGNSS(j, 2)*deg2rad;
        lastPosIMU(3) = measureGNSS(j, 3);
        
        % 记录
        measureZkLog(1:2, j) = measureZk(1:2, 1) / deg2rad;
        measureZkLog(3:4, j) = measureZk(3:4, 1);
        measureGNSSLog(1:2, j) = (measureGNSS(j, 1:2)-theoryTraj(i, 5:6))';
        measureGNSSLog(3:4, j) = (measureGNSS(j, 4:5)-theoryTraj(i, 8:9))';
        estimatePosLog(:, j) = estimateX(1:2, 1) / deg2rad;
        estimateVelLog(:, j) = estimateX(3:4, 1);
        estimateAngLog(:, j) = estimateX(5:7, 1) / deg2rad;
    end
end

%% 画图
figure;
hold on;
subplot(2, 1, 1);
plot(time, measureZkLog(1:2, :), 'LineWidth', 1.5);
legend('Lsins-Lgnss', 'Lambdasins-Lambdagnss');
grid on;
xlabel('time(s)');
ylabel('经纬度(°)');
title('IMU积分-GNSS测量');
subplot(2, 1, 2);
plot(time, measureZkLog(3:4, :), 'LineWidth', 1.5);
legend('VNsins-VNgnss', 'VEsins-VEgnss');
grid on;
xlabel('time(s)');
ylabel('速度(m/s)');
title('IMU积分-GNSS测量');

figure;
hold on;
subplot(2, 1, 1);
plot(time, measureGNSSLog(1:2, :), 'LineWidth', 1.5);
legend('LgnssError', 'LambdagnssError');
grid on;
xlabel('time(s)');
ylabel('经纬度(°)');
title('GNSS测量误差');
subplot(2, 1, 2);
plot(time, measureGNSSLog(3:4, :), 'LineWidth', 1.5);
legend('VEgnssError', 'VNgnssError');
grid on;
xlabel('time(s)');
ylabel('速度(m/s)');
title('GNSS测量误差');

figure;
hold on;
plot(time, estimatePosLog, 'LineWidth', 1.5);
grid on;
xlabel('time(s)');
ylabel('经纬度(°)');
title('SINS/GNSS融合导航估计误差');
legend('L', 'Lambda');

figure;
hold on;
plot(time, estimateVelLog, 'LineWidth', 1.5);
grid on;
xlabel('time(s)');
ylabel('速度(m/s)');
title('SINS/GNSS融合导航估计误差');
legend('Vn', 'Ve');

figure;
hold on;
plot(time, estimateAngLog, 'LineWidth', 1.5);
grid on;
xlabel('time(s)');
ylabel('欧拉角(°)');
title('SINS/GNSS融合导航估计误差');
legend('phiN', 'phiE', 'phiD');

%% 小函数
% PHI阵更新
function y = getPHI(Re, Omega, L, VE, VN, fE, fN, fD, dt)
    F = zeros(12,12);
    %F11\F12\0\0\0
    F(1:2,1:2) = [0                    0;
                 (VE/Re)*tan(L)*sec(L) 0];
    F(1:2,3:4) = [1/Re         0;
                  0    sec(L)/Re];
    %F21\F22\F23\I\0          
    F(3:4,1:2) = [-2*Omega*cos(L)*VE-(VE^2/Re)*(sec(L))^2   0;
                  2*Omega*cos(L)*VN+(VE*VN/Re)*(sec(L))^2   0];
    F(3:4,3:4) = [0              -2*Omega*sin(L)-(2*VE/Re)*tan(L);
                  (VN/Re)*tan(L)                                0];
    F(3:4,5:7) = [0 -fD fE;
                  fD 0 -fN];
    F(3:4,8:9) = eye(2);
    %F31\F32\F33\0\I
    F(5:7,1:2) = [-Omega*sin(L)                      0;
                  0                                  0;
                  -Omega*cos(L)-(VE/Re)*(sec(L))^2   0];
    F(5:7,3:4) = [0           1/Re;
                  -1/Re          0;
                  0     -tan(L)/Re];
    F(5:7,5:7) = [0                             -Omega*sin(L)-(VE/Re)*tan(L)              VN/Re;
                  Omega*sin(L)+(VE/Re)*tan(L)   0                            Omega*cos(L)+VE/Re;
                  -VN/Re                        -Omega*cos(L)-VE/Re                           0];
    F(5:7,10:12) = eye(3);
    % PHI
    y = expm(dt*F);
end

%姿态更新
function y = getAtt(angle, anglerate, dt)
    gamma = angle(1);
    psi = angle(2);
    theta = angle(3);
    dGamma = anglerate(1) - (anglerate(2)*cos(gamma)-anglerate(3)*sin(gamma))*tan(theta);
    dPsi = (anglerate(2)*cos(gamma)-anglerate(3)*sin(gamma))*sec(theta);
    dTheta = anglerate(2)*sin(gamma) - anglerate(3)*cos(gamma);
    
    y = zeros(3, 1);
    y(1) = gamma + dGamma*dt;
    y(2) = psi + dPsi*dt;
    y(3) = theta + dTheta*dt;
end

%速度更新
function y = getVel(fIn, posIn, velIn, dt)
    Re = 6378245;       %地球半径(m)
    OmegaIE = 7.292e-5; %地球自转角速度(rad/s)
    gE = 9.8;           %地球重力加速度(m/s^2)
    
    matGIn = [0; 0; gE];
    phi = posIn(1);
    Rm = Re * (1 -2*exp(1) +3*exp(1)*sin(phi)*sin(phi));
    Rn = Re * (1 +exp(1)*sin(phi)*sin(phi));
    matWenIn = [-velIn(2)/(Rm+posIn(3));
                velIn(1)/(Rn+posIn(3));
                velIn(1)/(Rn+posIn(3))*tan(phi)];
    matWieIn = [0; OmegaIE*cos(phi); OmegaIE*sin(phi)];
    
    dVelIn = fIn - cross((2*matWieIn+matWenIn), velIn) - matGIn;
    y = velIn + dVelIn*dt;
end

%位置更新
function y = getPos(posIn, velIn, velNew, dt)
    Re = 6378245;       %地球半径(m)
    
    phi = posIn(1);
    Rm = Re * (1 -2*exp(1) +3*exp(1)*sin(phi)*sin(phi));
    Rn = Re * (1 +exp(1)*sin(phi)*sin(phi));
    
    velIn = (velIn+velNew)/2;
    
    dPhi = velIn(2)/(Rm+posIn(3));
    dLambda = velIn(1)/((Rn+posIn(3))*cos(phi));
    dH = velIn(3);
    matD = [dPhi; dLambda; dH];
    
    y = posIn + matD*dt;
end

%本体系转到惯性系
function y = makeBtoI(x, angle)
    gamma = angle(1);
    psi = angle(2);
    theta = angle(3);
    matRx = [1,          0,           0;
             0, cos(gamma), -sin(gamma);
             0, sin(gamma),  cos(gamma)];
    matRy = [cos(psi),   0,    sin(psi);
             0,          1,           0;
             -sin(psi),  0,    cos(psi)];
    matRz = [cos(theta), -sin(theta), 0;
             sin(theta),  cos(theta), 0;
             0,           0,          1];
    matCIB = matRy * matRz * matRx;
    y = matCIB * x;
end
